#ifndef ICM_IMU_DRIVER_H
#define ICM_IMU_DRIVER_H

#include "main.h"
#include "spi.h"
#include "string.h"
#include "math.h"
#include "stdbool.h"
#include "icm_imu_define.h"
#include "log_system.h"

#define USE_FREERTOS

#ifdef USE_FREERTOS
#include "cmsis_os.h"
#define IMU_DELAY(t) osDelay(t)
#else
#define IMU_DELAY(t) HAL_Delay(t)
#endif

#define EACH_FRAME_LENGTH (12 + 4)
#define FRAME_MAX_CNT 4

#define G 9.80665f
#define MAX_INIT_PACK_NUM 30
#define MAX_OFFSET_DATA_CNT 3000

 #define ICM_DEVICE_ID 0x47
 #define ICM_SOFT_RESET 0x11
 #define ICM_INT_ENABLE_FIFO_WM   0x01
 #define ICM_FIFO_STREAM_MODE 0x40
 #define ICM_ACCEL_GYRO_MODE 0x0F
 #define INT_SOURCE0_FIFO_THS_FULL  ((1 << 2) | (1 << 1))
 #define INT_CONFIG_PUSH_PULL_HIGH_LATCHED  0x4F  // 0x0F | (1 << 6)

 #define ICM_DEVICE_ID_REG_ADDR 0x75
 #define ICM_DEVICE_CONFIG_REG_ADDR 0x11
 #define ICM_DEVICE_INT_CONFIG_REG_ADDR 0x14
 #define ICM_FIFO_CONFIG_REG_ADDR 0x16
 #define ICM_INT_STATUS_REG_ADDR 0x2D
 #define ICM_FIFO_COUNTH_REG_ADDR 0x2E
 #define ICM_FIFO_COUNTL_REG_ADDR 0x2F
 #define ICM_FIFO_DATA_REG_ADDR 0x30
 #define ICM_FIFO_CONFIG1_REG_ADDR 0x5F
 #define ICM_FIFO_CONFIG2_REG_ADDR 0x60
 #define ICM_FIFO_CONFIG3_REG_ADDR 0x61
 #define ICM_INT_SOURCE0_REG_ADDR 0x65
 #define ICM_INT_SOURCE1_REG_ADDR 0x66
 #define ICM_DEVICE_PWR_MGMT0_REG_ADDR 0x4E

typedef enum{
	SET_REG,
	GET_REG,
	SET_FROM_REG
}REG_OPERATE_WAY;

typedef struct{
	uint8_t init_reg;
	uint8_t from_reg;
	uint8_t value;
	uint16_t init_delay_time;
	REG_OPERATE_WAY way;
}Imu_Init_Pack;

typedef struct{
	Imu_Init_Pack init_packs[MAX_INIT_PACK_NUM];
	uint8_t now_num;
	uint8_t now_init_step;
}Imu_Init_list;

extern Imu_Init_list imu2_init_list, imu1_init_list;

void REGISTER_IMU_INIT_PACK(Imu_Init_list *imu_list, uint8_t init_reg, uint8_t from_reg, REG_OPERATE_WAY way, uint8_t init_value, uint16_t delay_time);

typedef enum{
	FIND_IMU,
	IMU_INIT,
	IMU_IDLE
}Imu_State;

typedef enum{
	FRAME_RECV,
	FRAME_WAIT,
	FRAME_PRASE
}Frame_Buffer_State;

typedef struct{
	Frame_Buffer_State buffer_state;
	uint8_t buffer[EACH_FRAME_LENGTH * FRAME_MAX_CNT];
	uint16_t buffer_count;
	uint16_t buffer_handle_length;
}Recv_Buffer;



typedef struct{
	int16_t ax;
	int16_t ay;
	int16_t az;
	int16_t gx;
	int16_t gy;
	int16_t gz;
}Imu_Frame_t;

typedef struct{
	float ax;
	float ay;
	float az;
	float gx;
	float gy;
	float gz;
}Imu_Senser_Original_Data;

typedef struct{
	uint8_t imu_id;
	SPI_HandleTypeDef* hspi_use;
	GPIO_TypeDef* CS_PORT;
	uint16_t CS_PIN;
	Imu_State imu_state;
	bool imu_online_if;
	bool fifo_ready;
	
	bool update_offset;
	uint16_t max_offset_data_get;
	uint16_t now_offset_data_get;
	
	
	uint8_t imu_reg_data[12];
	Imu_Frame_t now_imu_frame;
	Imu_Senser_Original_Data imu_original_data;
//	
//	uint8_t now_handle_frame_buffer_id;
//	uint8_t now_recv_frame_buffer_id;
//	Recv_Buffer recv_buffer[2];
	
	uint8_t gyro_max;
	float gyro_dps;
	uint8_t accel_max;
	float accel_dps;
	
	float get_gyro_x_data;
	float get_gyro_y_data;
	float get_gyro_z_data;
	
	float get_accel_x_data;
	float get_accel_y_data;
	float get_accel_z_data;
	
	float gyro_x_offset;
	float gyro_y_offset;
	float gyro_z_offset;
	
	float accel_x_offset;
	float accel_y_offset;
	float accel_z_offset;
}Imu_driver_t;


uint8_t IMU_SPI_ReadWriteBytes(Imu_driver_t *imu_driver, uint8_t addr, uint8_t txData, uint8_t *rxBuf, uint16_t len);
uint8_t set_icm_reg(Imu_driver_t *imu_driver, uint8_t reg_addr, uint8_t reg_value);
uint8_t get_icm_reg(Imu_driver_t *imu_driver, uint8_t reg_addr, uint8_t* buf, uint8_t read_size);
void ICM_ReadID(Imu_driver_t *imu_driver);
void ICM_Init(Imu_driver_t *imu_driver);
void imu_auto(Imu_driver_t *imu_driver);
void imu_struct_init(Imu_driver_t *imu_driver);

extern Imu_driver_t imu2;
extern Imu_driver_t imu1;

#endif
